//
// Created by whitby on 8/22/23.
//

#include "Map.h"

using namespace std;

namespace mono_orb_slam2 {

    void Map::addKeyFrame(const shared_ptr <KeyFrame> &keyFrame) {
        lock_guard<mutex> lock(map_mutex);
        keyframes.insert(keyFrame);
    }

    void Map::eraseKeyFrame(const shared_ptr <KeyFrame> &keyFrame) {
        lock_guard<mutex> lock(map_mutex);
        keyframes.erase(keyFrame);
    }

    void Map::addMapPoint(const shared_ptr <MapPoint> &mapPoint) {
        lock_guard<mutex> lock(map_mutex);
        map_points.insert(mapPoint);
    }

    void Map::eraseMapPoint(const shared_ptr <MapPoint> &mapPoint) {
        lock_guard<mutex> lock(map_mutex);
        map_points.erase(mapPoint);
    }

    vector <shared_ptr<KeyFrame>> Map::getRecentKeyFrames(const shared_ptr <KeyFrame> &curKeyFrame, int num) {
        lock_guard<mutex> lock(map_mutex);
        auto iterEnd = keyframes.find(curKeyFrame);
        iterEnd++;
        auto iterStart = iterEnd;
        while (num-- > 0 && iterStart != keyframes.begin())
            iterStart--;
        return {iterStart, iterEnd};
    }

    int Map::getNumKeyFrames() {
        lock_guard<mutex> lock(map_mutex);
        return (int) keyframes.size();
    }

    int Map::getNumMapPoints() {
        lock_guard<mutex> lock(map_mutex);
        return (int) map_points.size();
    }

    vector <shared_ptr<KeyFrame>> Map::getAllKeyFrames() {
        lock_guard<mutex> lock(map_mutex);
        return {keyframes.begin(), keyframes.end()};
    }

    std::vector<std::shared_ptr<MapPoint>> Map::getAllMapPoints() {
        lock_guard<mutex> lock(map_mutex);
        return {map_points.begin(), map_points.end()};
    }

    void Map::setReferenceMapPoints(const std::vector<std::shared_ptr<MapPoint>> &mapPoints) {
        lock_guard<mutex> lock(map_mutex);
        reference_map_points = mapPoints;
    }

    std::set<std::shared_ptr<MapPoint>> Map::getReferenceMapPoints() {
        lock_guard<mutex> lock(map_mutex);
        if (reference_map_points.empty()) {
            return map_points;
        }
        return {reference_map_points.begin(), reference_map_points.end()};
    }

    void Map::clear() {
        map_points.clear();
        keyframes.clear();
    }
} // mono_orb_slam2